#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/Twist.h"
#include <geometry_msgs/PolygonStamped.h>
#include "sstream"
#include "Pid.h"

using namespace std;
using namespace PID;
geometry_msgs::PointStamped base_point_left;
geometry_msgs::PointStamped base_point_right;
geometry_msgs::Twist msg;
PidCore m_pid = PidCore(0.0,0.5, 1.5,0.0);
unsigned char m_pubFlar = 0;

void transformPoint(const tf::TransformListener& listener)
{
    bool beamFlagLeft;
    bool beamFlagRight;
    geometry_msgs::PointStamped laser_point;
    laser_point.header.frame_id = "odom";
    laser_point.header.stamp = ros::Time();
    laser_point.point.x = 0.0;
    laser_point.point.y = 0.0;
    laser_point.point.z = 0.0;
    try
    {

        listener.transformPoint("base_left", laser_point, base_point_left);
        
        ROS_INFO("odom: (%.2f, %.2f. %.2f) -----> base_left: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point_left.point.x, base_point_left.point.y, base_point_left.point.z, base_point_left.header.stamp.toSec());
        
        listener.transformPoint("base_right", laser_point, base_point_right);
   
        ROS_INFO("odom: (%.2f, %.2f. %.2f) -----> base_right: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point_right.point.x, base_point_right.point.y, base_point_right.point.z, base_point_right.header.stamp.toSec());

        m_pid.Motor_Ctl(m_pid, base_point_left.point.x, base_point_left.point.y, base_point_left.point.z, 
                               base_point_right.point.x, base_point_right.point.y, base_point_right.point.z);
 
        msg.linear.x =  m_pid.m_pidLinevelocity;
        msg.angular.z = m_pid.m_pidAngularvelocity;
    }
    catch(tf::TransformException& ex)
    {

        ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_left\": %s", ex.what());
    }

}

int main(int argc, char** argv)
{   
    
    ros::init(argc, argv, "robot_tf_listener");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(10));
    ros::Timer timer = n.createTimer(ros::Duration(1), boost::bind(&transformPoint, boost::ref(listener)));
    
    ros::init(argc, argv, "ControlTurtlebot");
    ros::NodeHandle n_pub;
    ros::Publisher chatter_pub = n_pub.advertise<geometry_msgs::Twist>
    ("/cmd_vel_mux/input/teleop",10);

    ros::init(argc, argv, "PolygonStamped");
    ros::NodeHandle n_Polygon;
    ros::Publisher chatter_pubPolygon1 = n_Polygon.advertise<geometry_msgs::PolygonStamped>("/PolygonStamped/left",10);
    ros::Publisher chatter_pubPolygon2 = n_Polygon.advertise<geometry_msgs::PolygonStamped>("/PolygonStamped/right",10);
    geometry_msgs::PolygonStamped msg_Polygon1, msg_Polygon2;
    msg_Polygon1.header.frame_id = "base_left";
    msg_Polygon2.header.frame_id = "base_right";
	geometry_msgs::Point32 p;
	{
	    p.x = 0;
	    p.y = 0;	
	    p.z = 0;	
	    msg_Polygon1.polygon.points.push_back(p);
	    msg_Polygon2.polygon.points.push_back(p);
	}
	{
	    p.x = 1;
	    p.y = 1;	
	    p.z = 0;	
	    msg_Polygon1.polygon.points.push_back(p);
	    msg_Polygon2.polygon.points.push_back(p);
	}
	{
	    p.x = 1;
	    p.y = -1;	
	    p.z = 0;	
	    msg_Polygon1.polygon.points.push_back(p);
	    msg_Polygon2.polygon.points.push_back(p);
	}

        msg.linear.y = 0;
        msg.linear.z = 0;
        msg.angular.x = 0;
        msg.angular.y = 0;

    while(ros::ok())
    {
      
        msg_Polygon1.header.stamp = ros::Time::now();
        msg_Polygon2.header.stamp = ros::Time::now();
	    chatter_pubPolygon1.publish(msg_Polygon1);
	    chatter_pubPolygon2.publish(msg_Polygon2);
        chatter_pub.publish(msg);
        ros::spinOnce(); 
         
    }

}


